using System;
using System.Globalization;

namespace Atomic.Libraries.Mathematics
{
	public static class RootFinder
	{
		/// <summary>
		/// Solves f(x)=0 for a<=x<=b to machine precision, assuming a continuous function f. One of f(a) and f(b) must be positive and the other negative.
		/// </summary>
		public static double Bisection(Func<double, double> f, double a, double b)
		{
			return Bisection(f, a, b, f(a), f(b));
		}

		/// <summary>
		/// Solves f(x)=0 for a<=x<=b through a maximum number of iterations n, assuming a continuous function f. One of f(a) and f(b) must be positive and the other negative.
		/// </summary>
		public static double Bisection(Func<double, double> f, double a, double b, int n)
		{
			return Bisection(f, a, b, f(a), f(b), n);
		}

		public static double Bisection(Func<double, double> f, double a, double b, double fa, double fb)
		{
			return Bisection(f, a, b, fa, fb, 0);
		}

		public static double Bisection(Func<double, double> f, double a, double b, double fa, double fb, int n)
		{
			// http://en.wikipedia.org/w/index.php?title=Bisection_method&oldid=422656767#Practical_considerations

			if (f == null)
			{
				throw new ArgumentNullException();
			}

			// n=0 means no limit in number of iterations, i.e. solving to machine precision.
			if (n < 0)
			{
				throw new ArgumentOutOfRangeException();
			}

			if (double.IsNaN(fa) || double.IsNaN(fb) || Math.Sign(fa) == Math.Sign(fb))
			{
				// One must be positive and the other negative. Check for NaN first, fails otherwise.
				return double.NaN;
			}

			if (fa > 0.0)
			{
				double c = a;
				a = b;
				b = c;
			}

			// Now f(a)<=0 and f(b)>=0.

			int i = 0;
			double x = a + 0.5 * (b - a);
			while (x != a && x != b)
			{
				double fx = f(x);

				if (double.IsNaN(fx))
				{
					// Don't known which interval to choose in this case.
					return double.NaN;
				}

				if (fx <= 0.0)
				{
					a = x;
				}
				else
				{
					b = x;
				}
				x = a + 0.5 * (b - a);

				if (++i == n)
				{
					// No more iterations allowed. Keep updated value as best guess. Never satisfied if n=0.
					return x;
				}
			}

			// Converged successfully.
			return x;
		}

		//public static double Bisection(Function f, double a, double b, double eps)
		//{
		//    // http://en.wikipedia.org/w/index.php?title=Bisection_method&oldid=422656767#Pseudocode
		//    throw new NotImplementedException();
		//}
	
		public static double BrentsMethod(Func<double, double> f, double a, double b, double tol)
		{
			// http://en.wikipedia.org/w/index.php?title=Brent%27s_method&oldid=589594776#Example_code

			double c = 0;
			double d = double.MaxValue;
 
			double fa = f(a);
			double fb = f(b);
 
			double fc = 0;
			double s = 0;
			double fs = 0;
 
			// if f(a) f(b) >= 0 then error-exit
			if (fa * fb >= 0)
			{
				if (fa < fb)
				{
					return a;
				}
				else
				{
					return b;
				}
			}
 
			// if |f(a)| < |f(b)| then swap (a,b) end if
			if (Math.Abs(fa) < Math.Abs(fb))
			{
				double tmp = a;
				a = b;
				b = tmp;
				tmp = fa;
				fa = fb;
				fb = tmp;
			}
 
			c = a;
			fc = fa;
			bool mflag = true;
			int i = 0;
 
			while (!(fb == 0) && (Math.Abs(a - b) > tol))
			{
				if ((fa != fc) && (fb != fc))
				{
                    // Inverse quadratic interpolation
					s = a * fb * fc / (fa - fb) / (fa - fc) + b * fa * fc / (fb - fa) / (fb - fc) + c * fa * fb / (fc - fa) / (fc - fb);
				}
				else
				{
                    // Secant Rule
					s = b - fb * (b - a) / (fb - fa);
				}
 
				double tmp2 = (3 * a + b) / 4;
				if ((!(((s > tmp2) && (s < b)) || ((s < tmp2) && (s > b)))) || (mflag && (Math.Abs(s - b) >= (Math.Abs(b - c) / 2))) || (!mflag && (Math.Abs(s - b) >= (Math.Abs(c - d) / 2))))
				{
					s = (a + b) / 2;
					mflag = true;
				}
				else
				{
					if ((mflag && (Math.Abs(b - c) < tol)) || (!mflag && (Math.Abs(c - d) < tol)))
					{
						s = (a + b) / 2;
						mflag = true;
					}
					else
					{
						mflag = false;
					}
				}
				fs = f(s);
				d = c;
				c = b;
				fc = fb;
				if (fa * fs < 0)
				{
					b = s;
					fb = fs;
				}
				else
				{
					a = s;
					fa = fs;
				}
 
				// if |f(a)| < |f(b)| then swap (a,b) end if
				if (Math.Abs(fa) < Math.Abs(fb))
				{
					double tmp = a;
					a = b;
					b = tmp;
					tmp = fa;
					fa = fb;
					fb = tmp;
				}
				i++;
				if (i > 1000)
				{
					throw new ArithmeticException(string.Format(CultureInfo.InvariantCulture, "Din't converge. Error is {0}.", fb));
				}
			}
			return b;
		}
	}
}
